%function ind = test()
clear

isDebug = 0;
isNewObs = 0;

% if ~isDebug
%     startBoat_old(2);
% end

format long
ultimateLon =-79.948777;
ultimateLat =40.436604;
precision = 2;
varagin = {ultimateLat, ultimateLon};
if ~isDebug
% sendCommand(2, 'goObsAvoidNaive', varagin);
urlread('http://192.168.1.102/cgi-bin/serverScript.pl', 'GET', {'command', 'goObsAvoidNaive', 'args', ...
    ['[' num2str(ultimateLat,10) ', ' num2str(ultimateLon,10) ']']});
         urlread('http://192.168.1.102/cgi-bin/serverScript.pl', 'GET', {'command', 'goToWaypoint', 'args', ...
             ['[' num2str(ultimateLat,10) ', ' num2str(ultimateLon,10) ', ' num2str(precision) ', ' num2str(0.5) ']']});
end
boat_x = [0 1];                       % define the end points of the stick figure
boat_y = [0 0];
desiredH_x = [1 20];
desiredH_y = [0 0];
pause(5);
interval = 0.00003;
margin = 0.0009;
interLat = 0;
interLon = 0;
STEP = 1;
isfirst = 1;
R = 6371000;
CC = 0.9;
FC = 0;
FCT = 1;
RC = 0;
RCT = 4;
isFirst = 1;
isCali = 1;
if isDebug
    fp = fopen('~/autoBoat/log/status.txt');
    c = fscanf(fp,'%s');
    fclose(fp);
else
    c = urlread('http://192.168.1.102/status.txt');
end
lat = str2double(myParse(c, 'lat'));

lon = str2double(myParse(c, 'lon'));
% lat = 40.436426;
% lon = -79.948956;
goalLat = 0;
goalLon = 0;

% calDistance=@(lon1,lat1,lon2,lat2) (acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon2-lon1))*R);
Xmin = min(lon, ultimateLon)-margin;
Xmax = max(lon, ultimateLon)+margin;
Ymin = min(lat, ultimateLat)-margin;
Ymax = max(lat, ultimateLat)+margin;

axis([Xmin Xmax Ymin Ymax]);
hold on;
M = ceil((Ymax - Ymin)/interval);
N = ceil((Xmax - Xmin)/interval);
map = zeros(M,N);

mapping=@(x, y) (floor((x - Xmin)/interval)* M + floor((y - Ymin)/interval));

endpoint = mapping(ultimateLon, ultimateLat);
map(endpoint) = 10;


obs(10) = struct( ...
    'startIndex',[], ...
    'startValue',[], ...  test
    'endIndex',[], ...
    'endValue',[]);

% startHDL = plot(lon,lat, 'x');
% startTxt = text(lon,lat, sprintf('Start'), ...
%     'Color',[0.2 0.2 0.2], 'FontSize',8, ...
%     'HorizontalAlignment','left', 'VerticalAlignment','top');
% endHDL = plot(goalLon,goalLat, 'x');
% endTxt = text(goalLon,goalLat, sprintf('end'), ...
%     'Color',[0.2 0.2 0.2], 'FontSize',8, ...
%     'HorizontalAlignment','left', 'VerticalAlignment','top');

pathHDL = line('XData',0, 'YData',0, 'Color','r', ...
    'Marker','d', 'MarkerSize',1, 'LineWidth',2);

obsHDL = plot(0, 0, 'd');
        
boatHDL=plot(boat_x,boat_y,'EraseMode','background'); % connect the end points,
                                               % and use "EraseMode" to REDUCE FLASHING
desiredHHDL = plot(desiredH_x, desiredH_y, '--','EraseMode','background'); 

headHDL = line('XData',boat_x(1), 'YData',boat_y(1), 'Color','b', ...
    'Marker','o', 'MarkerSize',6, 'LineWidth',2);
headTxt = text(boat_x(1), boat_y(1), sprintf('(%.3f,%.3f)',boat_x(1),boat_y(1)), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

goalHDL = line('XData',ultimateLon, 'YData',ultimateLat, 'Color','r', ...
    'Marker','d', 'MarkerSize',6, 'LineWidth',2);
goalTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

interHDL = plot(0,0,'d');
interTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

obsDHDL = line('XData',0, 'YData',0, 'Color','k', ...
    'Marker','p', 'MarkerSize',6, 'LineWidth',2);
obsTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

set(gca,'DataAspectRatio',[1 1 1]);            % make x,y-axis relative scaling equal
xlabel('Longitude');
ylabel('Latitude');


while 1
    if isDebug
        fp = fopen('~/autoBoat/log/status.txt');
        c = fscanf(fp,'%s');
        fclose(fp);
    else
        c = urlread('http://192.168.1.102/status.txt');
    end
    lat =  str2double(myParse(c, 'lat'));
    lon =  str2double(myParse(c, 'lon'));
    
    if isnan(lat)
        continue;
    end
    
    indexDiff = abs(mapping(lon, lat) - mapping(ultimateLon, ultimateLat));
    
    if indexDiff == 0
        disp('Goal Reached!');
        if ~isDebug
            killBoat(2);
        end
        break;
    end
    

    
    goalLat =  str2double(myParse(c, 'goalLat'));
    goalLon =  str2double(myParse(c, 'goalLon'));


    start = mapping(lon, lat);
    map(start) = 1;


    obsLat =  str2double(myParse(c, 'obsLat'));
    obsLon =  str2double(myParse(c, 'obsLon'));
    
    set(goalHDL, 'XData',ultimateLon, 'YData',ultimateLat);
    set(goalTxt, 'Position',[ultimateLon ultimateLat], ...
        'String',sprintf('Goal(%.5f,%.5f)',[ultimateLon ultimateLat]));   
    set(obsDHDL, 'XData',obsLon, 'YData',obsLat);
    set(obsTxt, 'Position',[obsLon obsLat], ...
        'String',sprintf('Obstacle(%.5f,%.5f)',[obsLon obsLat]));
    
    compass = str2double(myParse(c, 'compass'));
    compass = compass/180*pi;
    if isDebug
        obsFile = fopen('~/autoBoat/log/obs.txt');
        obsC = fscanf(obsFile,'%s');
        fclose(obsFile);
    else
        obsC = urlread('http://192.168.1.102/obs.txt');
    end
    
    obsV = [];
    
    % FETCH SICK DATA %
     for i = 1: 181
         value = str2double(myParse(obsC, strcat('value', num2str(i-1))));
         obsV = [obsV, value];
     end
           
     if isFirst
         obsVL = obsV;
         isFirst = 0;
     end
     
     if isCali
         corrM = corrcoef(obsV, obsVL);
         if abs(corrM(2)) > CC
             disp('Similar!');
             FC = FC + 1;
             if FC > FCT
                 isCali = 0;
                 obsVR = obsV;
             end
             
         else
             FC = 0;
         end
         obsVL = obsV;
         continue;
     else
         
         % correlation coefficient
         
         corrM = corrcoef(obsV, obsVR);
         if abs(corrM(2)) > CC
             
             % remove individual points
%              
%              counter = 1;
%              
%              for k = 1 : 180
%                  if abs(obsV(k+1) - obsV(k)) > 100
%                      if counter < 10
%                          for m = k-counter+1 : k
%                              if m > 3
%                                  obsV(m) = (obsV(m-3)+obsV(m-2)+obsV(m-1))/3;
%                              else
%                                  obsV(m) = 8200;
%                              end
%                          end
%                          
%                      end
%                      counter = 1;
%                  else
%                      counter = counter + 1;
%                  end
%              end
%              
%              if counter < 10
%                  for m = 182-counter : 181
%                      obsV(m) = (obsV(m-3)+obsV(m-2)+obsV(m-1))/3;
%                  end
%              end
             
             % individual filter end %
             
             disp('refresh...');
             obsVL = obsV;
             obsVR = obsV;
         else
             
             disp('retain...');
             corrM = corrcoef(obsV, obsVL);
             if abs(corrM(2)) > CC
                 RC = RC + 1;
                 if RC > RCT
                     obsVR = obsV;
                 end
             end
             
             obsVL = obsV;
         end
         
         % PARSE SICK DATA %
         for i = 1: 181
             value = obsV(i);
             if value > 8182
                 continue
             else
                 tempI = 90.0 / 180 * i+45;
                 tempI = (tempI*pi)/180;
                 tempX = cos(tempI)*value;
                 tempY = sin(tempI)*value;
                 bearing = atan(tempX/tempY);
                 bearing = compass + bearing;
                 distance = sqrt((tempX/100)^2+(tempY/100)^2);
                 obsLat = asin(sin(lat*pi/180)*cos(distance/R) + cos(lat*pi/180)*sin(distance/R)*cos(bearing));
                 obsLon = lon + 180/pi*atan2(sin(bearing)*sin(distance/R)*cos(lat*pi/180),cos(distance/R)-sin(lat*pi/180)*sin(obsLat));
                 obsLat = obsLat *180 / pi;
                 if map(mapping(obsLon, obsLat)) ~= 9
                     isNewObs = 1;
                     map(mapping(obsLon, obsLat))=9;
                 end
             end

             
         end
         
         [obsY, obsX] = find(map == 9);
         obsY = Ymin + obsY*interval;
         obsX = Xmin + obsX*interval;
         set(obsHDL, 'XData', obsX, 'YData', obsY);
     end
    
    % Virtual Obstacle %
%     if lat > ultimateLat + interval*5
%         for o = ultimateLon-interval*5:interval: ultimateLon+interval*5
%             map(mapping(o, ultimateLat + interval*5)) = 9;
%         end
%     end
      
% %    map = fillmap(map, M, N);
%     [obsY, obsX] = find(map == 9);
%     obsY = Ymin + obsY*interval;
%     obsX = Xmin + obsX*interval;
%     set(obsHDL, 'XData', obsX, 'YData', obsY);

    
    if (isfirst)
        [path_x,path_y] = Astar(map,M,N,start,endpoint,Ymax,Ymin,Xmin);
        if length(path_x) > STEP
            interLat = path_y(STEP);
            interLon = path_x(STEP);
        else
            interLat = ultimateLat;
            interLon = ultimateLon;
        end
        
        crrLon = lon;
        crrLat = lat;
        
        isfirst = 0;
        varagin = {interLat, interLon};
        if ~isDebug
%         sendCommand(2, 'goObsAvoidNaive', varagin);
        urlread('http://192.168.1.102/cgi-bin/serverScript.pl', 'GET', {'command', 'goObsAvoidNaive', 'args', ...
            ['[' num2str(double(interLat),10) ', ' num2str(double(interLon),10) ']']});
         
%          urlread('http://192.168.1.102/cgi-bin/serverScript.pl', 'GET', {'command', 'goToWaypoint', 'args', ...
%              ['[' num2str(interLat,10) ', ' num2str(interLon,10) ', ' num2str(precision) ', ' num2str(0.5) ']']});
        end
    end
    
    
    if isNewObs
        disp('Got new obstacles...');
        isNewObs = 0;
        [path_x,path_y] = Astar(map,M,N,start,endpoint,Ymax,Ymin,Xmin);
        if length(path_x) > STEP
            interLat = path_y(STEP);
            interLon = path_x(STEP);
        else
            interLat = ultimateLat;
            interLon = ultimateLon;
        end
        
        crrLon = lon;
        crrLat = lat;
        
        isfirst = 0;
        varagin = {interLat, interLon};
        if ~isDebug
%         sendCommand(2, 'goObsAvoidNaive', varagin);
        urlread('http://192.168.1.102/cgi-bin/serverScript.pl', 'GET', {'command', 'goObsAvoidNaive', 'args', ...
            ['[' num2str(double(interLat),10) ', ' num2str(double(interLon),10) ']']});

        end
    end
    
    
    
%     if abs(lon - crrLon) > STEP*interval || abs(lat - crrLat) > STEP*interval
%         crrLon = lon;
%         crrLat = lat;
    indexDiff = abs(mapping(lon, lat) - mapping(interLon, interLat));
    
    if (indexDiff > 1) && (abs(indexDiff - M) > 1)
        disp('Recalculating due to deviation....');
        [path_x,path_y] = Astar(map,M,N,start,endpoint,Ymax,Ymin,Xmin);
        
        if length(path_x) > STEP
            interLat = path_y(STEP);
            interLon = path_x(STEP);
        else
            interLat = ultimateLat;
            interLon = ultimateLon;
        end
        varagin = {interLat, interLon};
        if ~isDebug
%         sendCommand(2, 'goObsAvoidNaive', varagin);
        urlread('http://192.168.1.102/cgi-bin/serverScript.pl', 'GET', {'command', 'goObsAvoidNaive', 'args', ...
            ['[' num2str(double(interLat),10) ', ' num2str(double(interLon),10) ']']});
%          urlread('http://192.168.1.102/cgi-bin/serverScript.pl', 'GET', {'command', 'goToWaypoint', 'args', ...
%              ['[' num2str(interLat,10) ', ' num2str(interLon,10) ', ' num2str(precision) ', ' num2str(0.5) ']']});
        end
    end
    
    started = str2double(myParse(c, 'started'));
    if started == 0
        
        disp('ACK...');
        [path_x,path_y] = Astar(map,M,N,start,endpoint,Ymax,Ymin,Xmin);
        
        if length(path_x) > STEP
            interLat = path_y(STEP);
            interLon = path_x(STEP);
        else
            interLat = ultimateLat;
            interLon = ultimateLon;
        end
        varagin = {interLat, interLon};
        if ~isDebug
%         sendCommand(2, 'goObsAvoidNaive', varagin);
         urlread('http://192.168.1.102/cgi-bin/serverScript.pl', 'GET', {'command', 'goObsAvoidNaive', 'args', ...
             ['[' num2str(double(interLat),10) ', ' num2str(double(interLon),10) ']']});
%          urlread('http://192.168.1.102/cgi-bin/serverScript.pl', 'GET', {'command', 'goToWaypoint', 'args', ...
%              ['[' num2str(interLat,10) ', ' num2str(interLon,10) ', ' num2str(precision) ', ' num2str(0.5) ']']});
        end
    end
    
       
%     set(startHDL, 'XData',path_x(1), 'YData',path_y(1));
% 
%     set(startTxt, 'Position',[path_x(1) path_y(1)], ...
%             'String',sprintf('AStar'));
% 
% 
%     set(endHDL, 'XData',path_x(end), 'YData',path_y(end));
% 
%     set(endTxt, 'Position',[path_x(end) path_y(end)], ...
%             'String',sprintf('End'));

    set(pathHDL,'XData',double(path_x),'YData',double(path_y));



    desiredH = str2double(myParse(c, 'desiredHeading'));
    desiredH = (90-desiredH)/180*pi;
    
    boat_y=[lat-0.0001*sin(pi/2-compass)  lat];
    boat_x=[lon-0.0001*cos(pi/2-compass)  lon];
    desiredH_y = [lat lat+0.0001*sin(desiredH)];
    desiredH_x = [lon lon+0.0001*cos(desiredH)];

    set(boatHDL,'XData',boat_x,'YData',boat_y);       % REDUCE FLASHING
    set(headHDL, 'XData',lon, 'YData',lat);
    set(headTxt, 'Position',[lon lat], ...
        'String',sprintf('Boat(%.5f,%.5f)',[lon lat]));
    set(desiredHHDL,'XData',desiredH_x,'YData',desiredH_y);
    
    iLatd = double(interLat);
    iLond = double(interLon);
    set(interHDL, 'XData',iLond, 'YData', iLatd);
    set(interTxt, 'Position',[iLond iLatd], ...
            'String',sprintf('Intermediate(%.5f,%.5f)',[iLond iLatd]));


    
    
    drawnow         % forces Matlab to redraw the plot at the current values
%    pause(.1)



end